#include <cmath>
#include <float.h>   // DBL_MAX
#include <string.h>  // memcpy() strncpy()
#include <unistd.h>  // unlink()
#include <stdio.h>
#include "DYN_toppra.hh"

typedef double M33[3][3];
typedef double M3[3];
typedef double M36[3][6];
typedef double M6[6];

#define pi 3.141592654

static double _alpha[6] = {0, pi / 2, 0, 0, pi / 2, -pi / 2};
static double _a[6] = {0, 0, 0, 0, 0, 0};
static double _d[6] = {0, 0, 0, 0, 0, 0};
static double _dynamicParameters[84] = {0};
static M33 _installMatrix = {0};

const static double _TSample = 0.008;

static double _ro_pl[6] = {0};
static double _ri_ph[6] = {0};
static double _ro_ph[6] = {0};
static double _u_p[6] = {0};
static double _y_p[6] = {0};
static double _r_est_p[6] = {0};

static double _ro_p[6] = {0};
static double _ro_pp[6] = {0};
static double _ri_p[6] = {0};

static int sign(double value)
{
    if (value > 0)
        return 1;
    else if (value < 0)
        return -1;
    else
        return 0;
}

static void multiple33_33(M33 A, M33 B, M33 AB)
{
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++) { AB[i][j] = A[i][0] * B[0][j] + A[i][1] * B[1][j] + A[i][2] * B[2][j]; }
    }
}

static void multiple36_6(M36 A, M6 B, M3 AB)
{
    for (int i = 0; i < 3; i++) { AB[i] = A[i][0] * B[0] + A[i][1] * B[1] + A[i][2] * B[2] + A[i][3] * B[3] + A[i][4] * B[4] + A[i][5] * B[5]; }
}

static void plus36_36(M36 A, M36 B, M36 AB)
{
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 6; j++) { AB[i][j] = A[i][j] + B[i][j]; }
    }
}

static void multiple33_36(M33 A, M36 B, M36 AB)
{
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 6; j++) { AB[i][j] = A[i][0] * B[0][j] + A[i][1] * B[1][j] + A[i][2] * B[2][j]; }
    }
}

static void multiple33_3(M33 A, M3 B, M3 AB)
{
    for (int i = 0; i < 3; i++) { AB[i] = A[i][0] * B[0] + A[i][1] * B[1] + A[i][2] * B[2]; }
}

static void cross(M3 A, M3 B, M3 AB)
{
    AB[0] = A[1] * B[2] - A[2] * B[1];
    AB[1] = -A[0] * B[2] + A[2] * B[0];
    AB[2] = A[0] * B[1] - A[1] * B[0];
}

static void plus33_33(M33 A, M33 B, M33 AB)
{
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++) { AB[i][j] = A[i][j] + B[i][j]; }
    }
}

static void plus3_3(M3 A, M3 B, M3 AB)
{
    for (int i = 0; i < 3; i++) { AB[i] = A[i] + B[i]; }
}

static void multiplek_3(double k, M3 B, M3 AB)
{
    for (int i = 0; i < 3; i++) { AB[i] = k * B[i]; }
}

static void multiplek_33(double k, M33 B, M33 AB)
{
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++) AB[i][j] = k * B[i][j];
    }
}

/*********************************************************************************************************
* Function name:        getT_toppra
* Descriptions:         get the desired torque
* input parameters:		q (dimension :6), qd(dimension :6), qdd(dimension :6), g
						q : angle of the joint 1-6
						qd : velocity of the joint 1-6
						qdd : acceleration  of the joint 1-6
						g : gravity
						dynamicParameters : 84 parameters
* output parameters:    t (dimension :6) : the torque applied on the joint
* Returned value:       1
*********************************************************************************************************/
static int getT_toppra(const double* q,
                       const double* qd,
                       const double* qdd,
                       double g,
                       const double* dynamicParameters,
                       double* torque,
                       double* fv,
                       double* fc,
                       double* offset)
{
    // unit vector of z axis
    M3 z0 = {0, 0, 1};
    // force
    M3 Fm[6];
    for (int i = 0; i < 6; i++)
        for (int j = 0; j < 3; j++) Fm[i][j] = 0.0;
    // torque
    M3 Nm[6];
    for (int i = 0; i < 6; i++)
        for (int j = 0; j < 3; j++) Nm[i][j] = 0.0;
    // p_star, the length between origin of j-1 coordinate and origin of j coordinate
    M3 p_star[7];
    for (int i = 0; i < 7; i++)
        for (int j = 0; j < 3; j++) p_star[i][j] = 0.0;
    // the transform matrix of j-1 coordinate to j coordinate
    M33 Tm[7];
    for (int i = 0; i < 7; i++)
        for (int j = 0; j < 3; j++)
            for (int k = 0; k < 3; k++)
            {
                Tm[i][j][k] = 0.0;
                if (j == k)
                    Tm[i][j][k] = 1.0;
            }

    // the rotation matirx of j to j-1
    M33 Rt[6];
    for (int i = 0; i < 6; i++)
        for (int j = 0; j < 3; j++)
            for (int k = 0; k < 3; k++)
            {
                Rt[i][j][k] = 0.0;
                if (j == k)
                    Rt[i][j][k] = 1.0;
            }
    // the angular velocity
    M3 angularV = {0.0, 0.0, 0.0};
    // the angular acceleration
    M3 angularA = {0.0, 0.0, 0.0};
    // the acceleration, the initial value is the gravity constant
    M3 acc0 = {0.0, 0.0, 9.81};
    M3 acc = {0.0, 0.0, 0.0};
    multiple33_3(_installMatrix, acc0, acc);
    if (g == 0)
    {
        for (int i = 0; i < 3; i++) { acc[i] = 0.0; }
    }

    //caculate the transform matirx and the p_star
    for (int i = 0; i < 6; i++)
    {
        Tm[i][0][0] = cos(q[i]);
        Tm[i][0][1] = -sin(q[i]);
        Tm[i][0][2] = 0.0;
        Tm[i][1][0] = sin(q[i]) * cos(_alpha[i]);
        Tm[i][1][1] = cos(q[i]) * cos(_alpha[i]);
        Tm[i][1][2] = -sin(_alpha[i]);
        Tm[i][2][0] = sin(q[i]) * sin(_alpha[i]);
        Tm[i][2][1] = cos(q[i]) * sin(_alpha[i]);
        Tm[i][2][2] = cos(_alpha[i]);
        p_star[i][0] = _a[i];
        p_star[i][1] = -sin(_alpha[i]) * _d[i];
        p_star[i][2] = cos(_alpha[i]) * _d[i];
        Rt[i][0][0] = cos(q[i]);
        Rt[i][1][0] = -sin(q[i]);
        Rt[i][2][0] = 0.0;
        Rt[i][0][1] = sin(q[i]) * cos(_alpha[i]);
        Rt[i][1][1] = cos(q[i]) * cos(_alpha[i]);
        Rt[i][2][1] = -sin(_alpha[i]);
        Rt[i][0][2] = sin(q[i]) * sin(_alpha[i]);
        Rt[i][1][2] = cos(q[i]) * sin(_alpha[i]);
        Rt[i][2][2] = cos(_alpha[i]);
    }
    //caculate the angular velocity angular acceleration acc Fm and Nm
    for (int i = 0; i < 6; i++)
    {
        M3 temp1, temp2, temp3, temp4, temp5, temp6, temp7;
        cross(angularV, p_star[i], temp1);
        cross(angularV, temp1, temp2);
        cross(angularA, p_star[i], temp3);
        plus3_3(temp2, temp3, temp4);
        plus3_3(temp4, acc, temp5);
        multiple33_3(Rt[i], temp5, acc);

        multiple33_3(Rt[i], angularA, temp1);
        multiplek_3(qdd[i], z0, temp2);
        multiple33_3(Rt[i], angularV, temp3);
        multiplek_3(qd[i], z0, temp4);
        cross(temp3, temp4, temp5);
        plus3_3(temp1, temp2, temp6);
        plus3_3(temp6, temp5, angularA);

        multiple33_3(Rt[i], angularV, temp1);
        multiplek_3(qd[i], z0, temp2);
        plus3_3(temp1, temp2, angularV);

        // obtain the stew matrix
        M33 sAngularV = {0, -angularV[2], angularV[1], angularV[2], 0, -angularV[0], -angularV[1], angularV[0], 0};
        M33 sAngularA = {0, -angularA[2], angularA[1], angularA[2], 0, -angularA[0], -angularA[1], angularA[0], 0};
        M33 sAcc = {0, -acc[2], acc[1], acc[2], 0, -acc[0], -acc[1], acc[0], 0};
        M36 lAngularV = {
            angularV[0], angularV[1], angularV[2], 0, 0, 0, 0, angularV[0], 0, angularV[1], angularV[2], 0, 0, 0, angularV[0], 0, angularV[1], angularV[2]};
        M36 lAngularA = {
            angularA[0], angularA[1], angularA[2], 0, 0, 0, 0, angularA[0], 0, angularA[1], angularA[2], 0, 0, 0, angularA[0], 0, angularA[1], angularA[2]};

        M3 dynamicParameters_part1 = {
            dynamicParameters[40 + 4 * i - 3 - 1], dynamicParameters[40 + 4 * i - 3 - 1 + 1], dynamicParameters[40 + 4 * i - 3 - 1 + 2]};
        M6 dynamicParameters_part2;
        for (int j = 0; j < 6; j++) dynamicParameters_part2[j] = dynamicParameters[6 * i + 1 - 1 + j];
        M33 tem, tem1, tem2, tem3;
        multiple33_33(sAngularV, sAngularV, tem);
        plus33_33(sAngularA, tem, tem1);
        //multiple33_3(tem1, dynamicParameters_part1, tem2);
        //multiplek_3(dynamicParameters[40 + 4 * i - 1], acc, tem3);
        //plus3_3(tem, tem2, Fm[i]);
        multiple33_3(tem1, dynamicParameters_part1, Fm[i]);
        multiplek_3(dynamicParameters[40 + 4 * i - 1], acc, temp7);
        plus3_3(Fm[i], temp7, Fm[i]);

        M36 te, te1;
        multiple33_36(sAngularV, lAngularV, te);
        plus36_36(lAngularA, te, te1);
        multiple36_6(te1, dynamicParameters_part2, temp1);
        multiple33_3(sAcc, dynamicParameters_part1, temp2);
        multiplek_3(-1, temp2, temp3);
        plus3_3(temp1, temp3, Nm[i]);
    }
    // caculate the desired torque
    M3 f = {0, 0, 0};
    M3 n = {0, 0, 0};

    for (int i = 5; i >= 0; i--)
    {
        M33 R;
        for (int j = 0; j < 3; j++)
            for (int k = 0; k < 3; k++) R[j][k] = Tm[i + 1][j][k];
        M3 temp1, temp2, temp3, temp4;
        multiple33_3(R, n, temp1);
        multiple33_3(R, f, temp4);
        cross(p_star[i + 1], temp4, temp2);
        plus3_3(temp1, temp2, temp3);
        plus3_3(temp3, Nm[i], n);

        plus3_3(temp4, Fm[i], f);

        torque[i] =
            n[2] + dynamicParameters[60 + i] * qdd[i] + dynamicParameters[66 + i] * qd[i] + dynamicParameters[72 + i] * sign(qd[i]) + dynamicParameters[78 + i];

        fv[i] = dynamicParameters[66 + i] * qd[i];
        fc[i] = dynamicParameters[72 + i] * sign(qd[i]);
        offset[i] = dynamicParameters[78 + i];
    }
    return 1;
}

/*********************************************************************************************************
* Function name:        getTa_toppra
* Descriptions:         get the desired torque
* input parameters:		q (dimension :6), qd(dimension :6), qdd(dimension :6)
						q : angle of the joint 1-6
						qd : velocity of the joint 1-6
						qdd : acceleration  of the joint 1-6
* output parameters:    torque (dimension :6) : the torque applied on the joint
* Returned value:       1
*********************************************************************************************************/
int getTa_toppra(const double* q, const double* qd, const double* qdd, double* torque, double* fv, double* fc, double* offset)
{
    static int count;
    //init(toolParameters);+
    count++;
    // printf("calling getTa %d \n",count);
    double lq[6] = {0};
    double lqd[6] = {0};
    double lqdd[6] = {0};
    for (int i = 0; i < 6; i++)
    {
        lq[i] = q[i];
        lqd[i] = qd[i];
        lqdd[i] = qdd[i];
    }
    // printf("lq = %f %f %f %f %f %f\n", lq[0], lq[1], lq[2], lq[3], lq[4], lq[5]);
    getT_toppra(lq, lqd, lqdd, 9.81, _dynamicParameters, torque, fv, fc, offset);
    return 1;
}

/*********************************************************************************************************
* Function name:        init
* Descriptions:         set dynamic parameters
* input parameters:		tool parameters(dimension:4), DH(dimension:6), installMatrix(dimension:3)
						toolParameters: mass, x, y, z
						DH: d1, d4, d5, d6, a2, a3
						installRPY
* output parameters:
* Returned value:       1
*********************************************************************************************************/
void DynPara_init_toppra(const double* toolParameters, const double* installRPY, const double* dynP, const double* a, const double* d, const double* alpha)
{
    // printf("toolParameters = %f %f %f %f \n", toolParameters[0], toolParameters[1], toolParameters[2], toolParameters[3]);
    // // printf("DH = %f %f %f %f %f %f \n",DH[0],DH[1],DH[2],DH[3],DH[4],DH[5]);
    // printf("installRPY = %f %f %f \n", installRPY[0], installRPY[1], installRPY[2]);
    double mp = toolParameters[0];
    double mp_x = toolParameters[1];
    double mp_y = toolParameters[2];
    double mp_z = toolParameters[3];

    // initialize the DH parameters
    _a[0] = a[0];
    _a[1] = a[1];
    _a[2] = a[2];
    _a[3] = a[3];
    _a[4] = a[4];
    _a[5] = a[5];
    _d[0] = d[0];
    _d[1] = d[1];
    _d[2] = d[2];
    _d[3] = d[3];
    _d[4] = d[4];
    _d[5] = d[5];

    double dynamicParameters[84] = {0,
                                    0,
                                    0,
                                    0,
                                    0,
                                    dynP[0] + mp * pow(_a[2], 2) + mp * (pow(_a[3], 2) + pow(_d[3], 2)),

                                    dynP[1] - mp * pow(_a[2], 2),
                                    dynP[2],
                                    dynP[3] - mp * _a[2] * _d[3],
                                    0,
                                    dynP[4],
                                    dynP[5] + mp * pow(_a[2], 2),

                                    dynP[6] - mp * pow(_a[3], 2),
                                    dynP[7],
                                    dynP[8] - mp * _a[3] * _d[3],
                                    0,
                                    dynP[9],
                                    dynP[10] + mp * pow(_a[3], 2),

                                    dynP[11] + mp * pow(_d[4], 2),
                                    dynP[12],
                                    dynP[13],
                                    0,
                                    dynP[14],
                                    dynP[15] + mp * pow(_d[4], 2),

                                    dynP[16] + mp * pow(_d[5], 2) + 2 * (mp * mp_z + 0.312 * mp_z + mp * (-0.037941)) * _d[5],
                                    dynP[17],
                                    dynP[18],
                                    0,
                                    dynP[19],
                                    dynP[20] + mp * pow(_d[5], 2) + 2 * (mp * mp_z + 0.312 * mp_z + mp * (-0.037941)) * _d[5],

                                    dynP[21],
                                    dynP[22],
                                    dynP[23],
                                    0,
                                    dynP[24],
                                    dynP[25],

                                    dynP[26],
                                    dynP[27] - mp * _d[3],
                                    0,
                                    0,
                                    dynP[28] + mp * _a[2],
                                    dynP[29],

                                    0,
                                    0,
                                    dynP[30] + mp * _a[3],
                                    dynP[31],
                                    0,
                                    0,

                                    dynP[32],
                                    dynP[33] - _d[4] * mp,
                                    0,
                                    0,
                                    dynP[34],
                                    dynP[35] + (mp * mp_z + 0.312 * mp_z + mp * (-0.037941)) + mp * _d[5],

                                    0,
                                    0,
                                    dynP[36] + (mp * mp_x + 0.312 * mp_x + mp * 0.43 / 1000),
                                    dynP[37] + (mp * mp_y + 0.312 * mp_y + mp * 0.605 / 1000),
                                    0,
                                    0,

                                    0,
                                    0,
                                    dynP[38],
                                    dynP[39],
                                    dynP[40],
                                    dynP[41],

                                    dynP[42],
                                    dynP[43],
                                    dynP[44],
                                    dynP[45],
                                    dynP[46],
                                    dynP[47],

                                    dynP[48],
                                    dynP[49],
                                    dynP[50],
                                    dynP[51],
                                    dynP[52],
                                    dynP[53],

                                    dynP[54],
                                    dynP[55],
                                    dynP[56],
                                    dynP[57],
                                    dynP[58],
                                    dynP[59]};

    for (int i = 0; i < 84; i++) { _dynamicParameters[i] = dynamicParameters[i]; }

    _alpha[0] = alpha[0];
    _alpha[1] = alpha[1];
    _alpha[2] = alpha[2];
    _alpha[3] = alpha[3];
    _alpha[4] = alpha[4];
    _alpha[5] = alpha[5];

    // initialize the gravity constant
    double x = installRPY[0];
    double y = installRPY[1];
    double z = installRPY[2];
    _installMatrix[0][0] = cos(z);
    _installMatrix[0][1] = sin(z) * cos(x);
    _installMatrix[0][2] = sin(z) * sin(x);
    _installMatrix[1][0] = -sin(z);
    _installMatrix[1][1] = cos(z) * cos(x);
    _installMatrix[1][2] = cos(z) * sin(x);
    _installMatrix[2][0] = 0;
    _installMatrix[2][1] = -sin(x);
    _installMatrix[2][2] = cos(x);
}